//卡尔曼滤波实验
//https://blog.csdn.net/sin1111yi/article/details/121871559
#include <QCoreApplication>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#define Kk_calc(x,y)    (x)/(x+y)

struct KalmanFilter
{
    float x_mea; // 测量值，不是随机值
    float x_est; // 估计值
    float e_mea; // 测量偏移，不能删除
    float e_est; // 估计偏移量
    float Kk; // 卡尔曼滤波增益
};

float RandomNumGenerator(int base, int range)
{
    float k = 0.0;
    float randomNum = 0.0;
    k = 2 * range * 10;
    randomNum = rand() % (int)k;
    k = base - range + (randomNum / 10);
    return k;
}

void BoostRandomNumGenerator()
{
    srand((unsigned)time(NULL));
}

void Kalman_Init(KalmanFilter *kalmanFilter, float FirstMeaValue, float E_mea, float FirstEstValue, float E_est)
{
    kalmanFilter->x_est = FirstEstValue;//估计值，滤波后的值
    kalmanFilter->x_mea = FirstMeaValue;//测量值
    kalmanFilter->e_est = E_est;//估计误差，每次进行估计后需要更新
    kalmanFilter->e_mea = E_mea;//固有的测量误差，取决于测量工具的精度，假设测量工具量程是2000/%2，测量误差就是2000*2%=40
    kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);//卡尔曼增益
}
//更新计算结果
void Kalman_Update(KalmanFilter *kalmanFilter, float newMeaValue)
{
    float temp = kalmanFilter->e_est;
    kalmanFilter->x_est = kalmanFilter->x_est + kalmanFilter->Kk * (newMeaValue - kalmanFilter->x_est);
    kalmanFilter->x_mea = newMeaValue;
    kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);
    kalmanFilter->e_est = (1 - kalmanFilter->Kk) * temp;
}


void test01()
{
    KalmanFilter k;
    BoostRandomNumGenerator();
    Kalman_Init(&k, 51.0, 3.0, 40, 5);
    for (int i = 0; i < 10; i++)
    {
        // Ten iterations
        Kalman_Update(&k, RandomNumGenerator(50, 3));
        printf("%.3f | %.3f\n", k.x_mea, k.x_est);
    }
}
#define DATA_LENGTH 100//定义数据缓冲区长度
void test02()
{
    float data[DATA_LENGTH];//数据缓冲区
    KalmanFilter k;
    BoostRandomNumGenerator();//随机种子发生器
    for (int i = 0; i < DATA_LENGTH; i++)
    {
        data[i] = RandomNumGenerator(50, 3);
    }
    //Kalman_Init(&k, 51.0, 3.0, 40, 5);
    Kalman_Init(&k, data[0], data[0], data[0] - 2, data[0] - 5);
    for (int i = 0; i < DATA_LENGTH; i++)
    {
        Kalman_Update(&k, data[i]);
        printf("%.3f | %.3f\n", k.x_mea, k.x_est);
    }
}
int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);
    //test01();
    test02();
    return a.exec();
}
